/*! \file swing_up_noMotor.cpp

\brief Swing up controller for Cartpole pendulum system

Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.
*/
/*
INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param int i: Time flag
\param int ui: Real time

OUTPUT
\param double TAU: Torque input
\param VEC X: State derivate

*/

#include "../../dynlib.h"

void swing_up(Matrix &SimParam,Matrix &X,int i,int ui,double *Tau,ColumnVector &dX)
{
	/*swing_up(M,m,L,g,R,r,Km,Kg,teta,dteta,i):
	M=SimParam->me[1][1]
	m=SimParam->me[1][2]
	L=SimParam->me[1][3]
	g=SimParam->me[1][4]
	R=SimParam->me[2][1]
	r=SimParam->me[2][2]
	Km=SimParam->me[2][3]
	Kg=SimParam->me[2][4]*/

	//Sign of control action
	if (X(4,i)* cos(X(2,i)))>0.0
		*Tau=-3.5;
	else
		*Tau=3.5;
}